# 本类 参考了网络文章Jetson nano i2c教程（MPU6050 + PCA9685）
# https://blog.csdn.net/qq_18676517/article/details/104873374
# !/usr/bin/python

from pca9685_thread import PCA9685Thread
from rate_limiter import RateLimiter
import queue
import time
# ============================================================================
# Raspi PCA9685 16-Channel PWM Servo Driver
# ============================================================================


class PCA9685:
    rate_limiter = RateLimiter(15)

    def __init__(self, address=0x40, frequency=50, debug=False, dev_index=0):
        """
        初始化对象
        :param address: i2c地址
        :param frequency: PWM频率，默认50用于驱动舵机，如果驱动电机最好改为500HZ以上。
        :param debug: 是否显示调试信息。
        """
        self._pre_left = -1000
        self._pre_left_dir = True
        self._pre_right = -1000
        self._pre_right_dir = True
        self.work_queue = queue.Queue()
        self.pca_thread = PCA9685Thread(self.work_queue, address, frequency, debug, dev_index)
        self.debug = debug
        self.pca_thread.daemon = True
        self.pca_thread.start()

    def setPWM(self, channel, on, off):
        if self.work_queue.qsize() > 4:
            print("写入I2C模块超速，本次写入被放弃。")
        else:
            self.work_queue.put([channel, on, off])

    def _setServoPulse(self, channel, pulse):
        """
        Sets the Servo Pulse,The PWM frequency must be 50HZ
        :param channel:
        :param pulse:
        :return:
        """
        pulse = int(pulse * 4096 / 20000)  # PWM frequency is 50HZ,the period is 20000us
        self.setPWM(channel, 0, pulse)

    def _map(self, x, in_min=0, in_max=180, out_min=500, out_max=2500):
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    def _setMotoPluse(self, channel, pulse):
        """
        设置驱动马达的脉冲
        :param channel: 通道
        :param pulse: 脉冲（0~4095）
        :return:
        """
        if pulse > 4095:
            self.setPWM(channel, 0, 4095)
        else:
            self.setPWM(channel, 0, pulse)

    def set_servo_angle(self, channel, angle):
        """
        设置舵机角度
        :param channel:舵机的通道（4~15)
        :param angle: 角度（0~180)
        :return:
        """
        if self.debug:
            print("设置 {} 通道，舵机转动 {} 角度。".format(channel, angle))
        angle = 0 if angle < 0 else angle
        angle = 180 if angle > 180 else angle
        self._setServoPulse(channel, self._map(angle))

    def digital_write(self, channel, value):
        """
        指定引脚输出高低电平,模仿Arduino的digitalWrite
        由于jetson nano 的GPIO实在太弱，借用pca9685来输出高电平。
        :param channel:引脚
        :param value:输入为0，低电平，其它值为高电平。
        """
        if value == 0:
            self.setPWM(channel=channel, on=0, off=0)
        else:
            self.setPWM(channel=channel, on=0, off=4095)

    def analog_write(self, channel, value):
        """
        指定引脚输出PWM(范围是0~255)，模仿Arduino的analogWrite
        :param channel: 引脚(范围是0~15)
        :param value: pwm值（范围是0~255）
        """
        if self.debug:
            print("设置 {} 通道，输出PWM值 {} 。".format(channel, value))
        if value < 0:
            value = 0
        if value > 255:
            value = 255
        self._setMotoPluse(channel, int(abs(value) * 4095 / 255))

    def _set_motor(self, pin_dir, pin_pwm, speed, pre_speed, pre_dir):
        """
        设置单个马达的速度和方向。

        :param pin_dir: 马达方向控制引脚
        :param pin_pwm: 马达PWM控制引脚
        :param speed: 马达速度
        :param pre_speed: 前一个速度值
        :param pre_dir: 前一个方向值
        """
        if abs(speed - pre_speed) >= 2:
            # 设置方向
            if speed >= 0 and not pre_dir:
                self._setMotoPluse(pin_dir, 0)  # 正向
                pre_dir = True
            elif speed < 0 and pre_dir:
                self._setMotoPluse(pin_dir, 4095)  # 反向
                pre_dir = False

                # 设置PWM值
            self._setMotoPluse(pin_pwm, int(abs(speed) * 4095 / 255))

        return pre_speed, pre_dir

    @rate_limiter
    def drive_servo_car(self, left_pin, left_speed, right_pin, right_speed):
        """
        驱动双360度舵机小车
        :param left_pin: 左舵机控制引脚
        :param left_speed: 左舵机速度（-255~255）
        :param right_pin: 右舵机控制引脚
        :param right_speed: 右舵机速度（-255~255）
        """

        # 将速度转换为脉冲（500-2500）
        def speed_to_pulse(speed):
            pulse = int(speed * 1000 / 255) + 1500  # 将-255~255映射到500~2500
            return max(500, min(2500, pulse))  # 限制在有效范围

        # 左舵机正向控制
        left_pulse = speed_to_pulse(left_speed)
        # 右舵机反向控制（速度取反）
        right_pulse = speed_to_pulse(-right_speed)

        if self.debug:
            print(f"左舵机[{left_pin}]脉冲: {left_pulse} | 右舵机[{right_pin}]脉冲: {right_pulse}")

        # 发送脉冲信号
        self._setServoPulse(left_pin, left_pulse)
        self._setServoPulse(right_pin, right_pulse)

    @rate_limiter
    def drive_motor(self, left, right):
        """
        输入马达速度，驱动马达前进。

        :param left: 左马达速度（-255,255）
        :param right: 右马达速度（-255,255）
        """
        if self.debug:
            print("设置左马达速度：{}，设置右马达速度：{}".format(left, right))

            # 更新左马达的速度和方向
        self._pre_left, self._pre_left_dir = self._set_motor(
            0, 1, left, self._pre_left, self._pre_left_dir
        )

        # 更新右马达的速度和方向
        self._pre_right, self._pre_right_dir = self._set_motor(
            2, 3, right, self._pre_right, self._pre_right_dir
        )

    @rate_limiter
    def drive_one_motor(self, channel, speed):
        """
        驱动一个电机，没有方向，接一个通道，另一根线接地
        :param channel: 通道(0~15)
        :param speed: 速度（0~255）
        :return:
        """
        self.analog_write(channel, speed)

    def _set_motor_pulse(self, pin_1, pin_2, speed, pre_speed, pre_dir, max_pulse=4095):
        # 检查速度变化是否大于2
        if abs(speed - pre_speed) >= 2:
            # 根据速度的正负设置引脚
            if speed <= 0:
                pin_1, pin_2 = pin_2, pin_1
                if pre_dir:
                    self._setMotoPluse(pin_2, 0)
                    pre_dir = False
            else:
                if not pre_dir:
                    self._setMotoPluse(pin_2, 0)
                    pre_dir = True
                    # 设置马达脉冲
            self._setMotoPluse(pin_1, int(abs(speed) * max_pulse / 255))
            return speed, pre_dir
        return pre_speed, pre_dir

    @rate_limiter
    def drive_motor_2(self, left, right, pin_left_1=0, pin_left_2=1, pin_right_1=2, pin_right_2=3):
        """
        ... (函数文档保持不变)
        """

        if self.debug:
            print("设置左马达速度：{}，设置右马达速度：{}".format(left, right))

        self._pre_left, self._pre_left_dir = self._set_motor_pulse(
            pin_left_1, pin_left_2, left, self._pre_left, self._pre_left_dir
        )
        self._pre_right, self._pre_right_dir = self._set_motor_pulse(
            pin_right_1, pin_right_2, right, self._pre_right, self._pre_right_dir
        )

    def close_device(self):
        self.work_queue.put(None)
        self.pca_thread.close_device()
        time.sleep(0.1)

    def close(self):
        pass

    def drive_servo(self, angle):
        pass

    # @rate_limiter
    # def drive_motor(self, left, right):
    #     """
    #         输入马达速度，驱动马达前进，此函数用于驱动那种需要两路PWM,两路数字信号的板，例如L298P
    #         PCA9685的0，2脚对应左，右马达的方向，1,3对应左右的PWM
    #     :param left: 左马达速度（-255,255）
    #     :param right: 右马达速度（-255,255）
    #     """
    #     if self.debug:
    #         print("设置左马达速度：{}，设置右马达速度：{}".format(left, right))
    #
    #     if abs(left - self._pre_left) >= 2:
    #         if left >= 0 and not self._pre_left_dir:
    #             self._setMotoPluse(0, 0)
    #             self._pre_left_dir = True
    #         elif left <= 0 and self._pre_left_dir:
    #             self._setMotoPluse(0, 4095)
    #             self._pre_left_dir = False
    #         else:
    #             pass
    #
    #         self._setMotoPluse(1, int(abs(left) * 4095 / 255))
    #         self._pre_left = left
    #
    #     if abs(right - self._pre_right) >= 2:
    #         if right >= 0 and not self._pre_right_dir:
    #             self._setMotoPluse(2, 0)
    #             self._pre_right_dir = True
    #         elif right < 0 and self._pre_right_dir:
    #             self._setMotoPluse(2, 4095)
    #             self._pre_right_dir = False
    #         else:
    #             pass
    #
    #         self._setMotoPluse(3, int(abs(right) * 4095 / 255))
    #         self._pre_right = right

    # @rate_limiter
    # def drive_motor_2(self, left, right, pin_left_1=0, pin_left_2=1, pin_right_1=2, pin_right_2=3):
    #     """
    #     输入马达速度，驱动马达前进,此函数用于驱动需要四路PWM驱动的板，如L298N
    #         PCA9685的四个指定针脚连接驱动板对应的左右马达针脚
    #     :param left: 左马达速度（-255,255）
    #     :param right: 右马达速度（-255,255）
    #     :param pin_left_1: 连接左马达驱动针脚1
    #     :param pin_left_2: 连接左马达驱动针脚2
    #     :param pin_right_1: 连接右马达驱动针脚1
    #     :param pin_right_2: 连接右马达驱动针脚2
    #     :return:
    #     """
    #
    #     if self.debug:
    #         print("设置左马达速度：{}，设置右马达速度：{}".format(left, right))
    #
    #     if abs(left - self._pre_left) >= 2:
    #         if left <= 0:
    #             pin_left_1, pin_left_2 = pin_left_2, pin_left_1
    #             if self._pre_left_dir:
    #                 self._setMotoPluse(pin_left_2, 0)
    #                 self._pre_left_dir = False
    #         else:
    #             if not self._pre_left_dir:
    #                 self._setMotoPluse(pin_left_2, 0)
    #                 self._pre_left_dir = True
    #         self._setMotoPluse(pin_left_1, int(abs(left) * 4095 / 255))
    #         self._pre_left = left
    #
    #     if abs(right - self._pre_right) >= 2:
    #         if right <= 0:
    #             pin_right_1, pin_right_2 = pin_right_2, pin_right_1
    #             if self._pre_right_dir:
    #                 self._setMotoPluse(pin_right_2, 0)
    #                 self._pre_right_dir = False
    #         else:
    #             if self._pre_right_dir:
    #                 self._setMotoPluse(pin_right_2, 0)
    #                 self._pre_right_dir = True
    #         self._setMotoPluse(pin_right_1, int(abs(right) * 4095 / 255))
    #         self._pre_right = right




if __name__ == '__main__':
    import random
    pwm = PCA9685(0x40, debug=True)

    # while True:
    #     x = random.randint(-40, 255)
    #     print(x)
    #     pwm.drive_motor(x, x)
    #     time.sleep(0.17)
    # print("start the control")
    r = 0
    g = 0
    b = 1
    time.sleep(0.1)
    while True:
        r = not r
        g = not g
        b = not b
        pwm.digital_write(0, r)
        pwm.digital_write(1, g)
        pwm.digital_write(2, b)
        time.sleep(0.08)
        #
        # for i in range(2500, 500, -10):
        #     pwm.setServoPulse(0, i)
        #     time.sleep(0.02)
